/*
 *------------------------------------------------------------------------------
 * @File      :    mi_camera_ops.c
 * @Date      :    2021-3-16
 * @Author    :    lomboswer <lomboswer@lombotech.com>
 * @Brief     :    Source file for MDP(Media Development Platform).
 *
 * Copyright (C) 2020-2021, LomboTech Co.Ltd. All rights reserved.
 *------------------------------------------------------------------------------
 */
#if defined(__EOS__)
#include <rtdevice.h>
#else
#include <linux/i2c.h>
#include <linux/i2c-dev.h>
#include <linux/stat.h>
#include <fcntl.h>
#include <sys/time.h>
#include <sys/ioctl.h>
#include <sys/stat.h>
#endif
#include "camera_usr.h"
#include "ei_comm_camera.h"
#include <pthread.h>

#ifdef __cplusplus
#if __cplusplus
extern "C" {
#endif
#endif /* __cplusplus */

#define I2C_FUNC_CQI        0x00000100
#define I2CCQI_16BIT_VAL    0x0040
#define I2CCQI_16BIT_REG    0x0020
#define I2CCQI_USE          0x0080

/* pthread_mutex_t g_aMutex[5] = {PTHREAD_MUTEX_INITIALIZER}; */
pthread_mutex_t g_aMutex[5] = {[0 ...(4)] = PTHREAD_MUTEX_INITIALIZER};


static EI_S32 CAMERA_I2C_WriteR8V8(SNS_STATE_S *pstSnsState, EI_U32 Reg, EI_U32 Val)
{
    EI_S32 s32Ret = EI_SUCCESS;
#if defined(__EOS__)
    struct rt_i2c_msg msgs[2];

#else
    struct i2c_rdwr_ioctl_data stI2cData;
    struct i2c_msg stMsg = {0};
#endif
    EI_U8 Buf[2] = {(EI_U8)Reg, (EI_U8)Val};
#if 0
    pthread_mutex_lock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);
    if (pstSnsState->bPower == EI_FALSE) {
        pthread_mutex_unlock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);
        cprintf("%s sensor not power on!\n", pstSnsState->pcName);
        return -EI_FAILURE;
    }
#endif
#if defined(__EOS__)


    pthread_mutex_lock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);

    if (pstSnsState->bPower == EI_FALSE) {
        PRT_VISS_WARN("%s sensor not power on!\n", pstSnsState->pcName);
        pthread_mutex_unlock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);
        return -EI_FAILURE;
    }
    msgs[0].addr  = pstSnsState->stI2cInfo.u16I2cDevAddr;
    msgs[0].flags = RT_I2C_WR;
    msgs[0].buf   = Buf;
    msgs[0].len   = 2;

    if (rt_i2c_transfer(pstSnsState->stI2cInfo.i2c_bus, msgs, 1) == 1)
        s32Ret = EI_SUCCESS;
    else
        s32Ret = EI_FAILURE;


    pthread_mutex_unlock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);


#else
    pthread_mutex_lock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);
    if (pstSnsState->bPower == EI_FALSE) {
        pthread_mutex_unlock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);
        PRT_VISS_WARN("%s sensor not power on!\n", pstSnsState->pcName);
        return -EI_FAILURE;
    }
    stMsg.addr  = pstSnsState->stI2cInfo.u16I2cDevAddr;
    stMsg.flags = 0;
    stMsg.len   = 2;
    stMsg.buf   = Buf;

    stI2cData.msgs = &stMsg;
    stI2cData.nmsgs = 1;

    s32Ret = ioctl(pstSnsState->stI2cInfo.fd, I2C_RDWR, &stI2cData);
    pthread_mutex_unlock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);
#endif


    if (s32Ret < 0) {
        cprintf("%s ret=%d,reg=0x%04x,val=0x%04x\n",
            pstSnsState->pcName, s32Ret, Reg, Val);
        return s32Ret;
    }

    return EI_SUCCESS;
}


static EI_S32 CAMERA_I2C_ReadR8V8(SNS_STATE_S *pstSnsState, EI_U32 Reg, void *pVal)
{
    EI_S32 s32Ret = EI_SUCCESS;
#if defined(__EOS__)
    struct rt_i2c_msg msgs[2];

#else
    struct i2c_rdwr_ioctl_data stI2cData;
    struct i2c_msg stMsg[2] = {0};
#endif
#if 0
    pthread_mutex_lock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);
    if (pstSnsState->bPower == EI_FALSE) {
        pthread_mutex_unlock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);
        cprintf("%s sensor not power on!\n", pstSnsState->pcName);
        return -EI_FAILURE;
    }
#endif
#if defined(__EOS__)


    pthread_mutex_lock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);

    if (pstSnsState->bPower == EI_FALSE) {
        PRT_VISS_WARN("%s sensor not power on!\n", pstSnsState->pcName);
        pthread_mutex_unlock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);
        return -EI_FAILURE;
    }
    msgs[0].addr  = pstSnsState->stI2cInfo.u16I2cDevAddr;
    msgs[0].flags = RT_I2C_WR;
    msgs[0].buf   = (EI_U8 *)(&Reg);
    msgs[0].len   = 1;

    msgs[1].addr  = pstSnsState->stI2cInfo.u16I2cDevAddr;
    msgs[1].flags = RT_I2C_RD; /* Read from slave */
    msgs[1].buf   = (EI_U8 *)pVal;
    msgs[1].len   = 1;

    if (rt_i2c_transfer(pstSnsState->stI2cInfo.i2c_bus, msgs, 2) == 2)
        s32Ret = EI_SUCCESS;
    else
        s32Ret = EI_FAILURE;


    pthread_mutex_unlock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);


#else
    pthread_mutex_lock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);
    if (pstSnsState->bPower == EI_FALSE) {
        pthread_mutex_unlock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);
        PRT_VISS_WARN("%s sensor not power on!\n", pstSnsState->pcName);
        return -EI_FAILURE;
    }
    stMsg[0].addr  = pstSnsState->stI2cInfo.u16I2cDevAddr;
    stMsg[0].flags = 0;
    stMsg[0].len   = 1;
    stMsg[0].buf   = (EI_U8 *)(&Reg);

    stMsg[1].addr  = pstSnsState->stI2cInfo.u16I2cDevAddr;
    stMsg[1].flags |= I2C_M_RD;
    stMsg[1].len   = 1;
    stMsg[1].buf   = (EI_U8 *)pVal;

    stI2cData.msgs = stMsg;
    stI2cData.nmsgs = 2;

    s32Ret = ioctl(pstSnsState->stI2cInfo.fd, I2C_RDWR, &stI2cData);
    pthread_mutex_unlock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);
#endif

    if (s32Ret < 0) {
        cprintf("%s ret=%d,reg=0x%04x\n",
            pstSnsState->pcName, s32Ret, Reg);
        return s32Ret;
    }

    return EI_SUCCESS;
}

static EI_S32 CAMERA_I2C_WriteR16V8(SNS_STATE_S *pstSnsState, EI_U32 Reg, EI_U32 Val)
{
    EI_S32 s32Ret = EI_SUCCESS;
#if defined(__EOS__)
    struct rt_i2c_msg msgs[2];

#else
    struct i2c_rdwr_ioctl_data stI2cData;
    struct i2c_msg stMsg = {0};
#endif
    EI_U8 Buf[3] = {(EI_U8)(Reg >> 8), (EI_U8)(Reg & 0xff), (EI_U8)Val};
#if 0
    pthread_mutex_lock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);
    if (pstSnsState->bPower == EI_FALSE) {
        pthread_mutex_unlock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);
        cprintf("%s sensor not power on!\n", pstSnsState->pcName);
        return -EI_FAILURE;
    }
#endif
#if defined(__EOS__)


    pthread_mutex_lock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);

    if (pstSnsState->bPower == EI_FALSE) {
        PRT_VISS_WARN("%s sensor not power on!\n", pstSnsState->pcName);
        pthread_mutex_unlock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);
        return -EI_FAILURE;
    }
    msgs[0].addr  = pstSnsState->stI2cInfo.u16I2cDevAddr;
    msgs[0].flags = RT_I2C_WR;
    msgs[0].buf   = Buf;
    msgs[0].len   = 3;

    if (rt_i2c_transfer(pstSnsState->stI2cInfo.i2c_bus, msgs, 1) == 1)
        s32Ret = EI_SUCCESS;
    else
        s32Ret = EI_FAILURE;


    pthread_mutex_unlock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);


#else
    pthread_mutex_lock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);
    if (pstSnsState->bPower == EI_FALSE) {
        pthread_mutex_unlock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);
        PRT_VISS_WARN("%s sensor not power on!\n", pstSnsState->pcName);
        return -EI_FAILURE;
    }
    stMsg.addr  = pstSnsState->stI2cInfo.u16I2cDevAddr;
    stMsg.flags = 0;
    stMsg.len   = 3;
    stMsg.buf   = Buf;

    stI2cData.msgs = &stMsg;
    stI2cData.nmsgs = 1;

    s32Ret = ioctl(pstSnsState->stI2cInfo.fd, I2C_RDWR, &stI2cData);
    pthread_mutex_unlock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);
#endif
    if (s32Ret < 0) {
        cprintf("%s ret=%d,reg=0x%04x,val=0x%04x\n",
            pstSnsState->pcName, s32Ret, Reg, Val);
        return s32Ret;
    }

    return EI_SUCCESS;
}

static EI_S32 CAMERA_I2C_ReadR16V8(SNS_STATE_S *pstSnsState, EI_U32 Reg, void *pVal)
{
    EI_S32 s32Ret = EI_SUCCESS;
#if defined(__EOS__)
    struct rt_i2c_msg msgs[2];

#else
    struct i2c_rdwr_ioctl_data stI2cData;
    struct i2c_msg stMsg[2] = {0};
#endif
    EI_U8 Buf[2] = {(EI_U8)(Reg >> 8), (EI_U8)(Reg & 0xff)};
#if 0
    pthread_mutex_lock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);
    if (pstSnsState->bPower == EI_FALSE) {
        pthread_mutex_unlock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);
        cprintf("%s sensor not power on!\n", pstSnsState->pcName);
        return -EI_FAILURE;
    }
#endif
#if defined(__EOS__)


    pthread_mutex_lock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);

    if (pstSnsState->bPower == EI_FALSE) {
        PRT_VISS_WARN("%s sensor not power on!\n", pstSnsState->pcName);
        pthread_mutex_unlock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);
        return -EI_FAILURE;
    }
    msgs[0].addr  = pstSnsState->stI2cInfo.u16I2cDevAddr;
    msgs[0].flags = RT_I2C_WR;
    msgs[0].buf   = Buf;
    msgs[0].len   = 2;

    msgs[1].addr  = pstSnsState->stI2cInfo.u16I2cDevAddr;
    msgs[1].flags = RT_I2C_RD; /* Read from slave */
    msgs[1].buf   = (EI_U8 *)pVal;
    msgs[1].len   = 1;

    if (rt_i2c_transfer(pstSnsState->stI2cInfo.i2c_bus, msgs, 2) == 2)
        s32Ret = EI_SUCCESS;
    else
        s32Ret = EI_FAILURE;


    pthread_mutex_unlock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);


#else
    pthread_mutex_lock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);
    if (pstSnsState->bPower == EI_FALSE) {
        pthread_mutex_unlock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);
        PRT_VISS_WARN("%s sensor not power on!\n", pstSnsState->pcName);
        return -EI_FAILURE;
    }
    stMsg[0].addr  = pstSnsState->stI2cInfo.u16I2cDevAddr;
    stMsg[0].flags = 0;
    stMsg[0].len   = 2;
    stMsg[0].buf   = Buf;

    stMsg[1].addr  = pstSnsState->stI2cInfo.u16I2cDevAddr;
    stMsg[1].flags |= I2C_M_RD;
    stMsg[1].len   = 1;
    stMsg[1].buf   = (EI_U8 *)pVal;

    stI2cData.msgs = stMsg;
    stI2cData.nmsgs = 2;

    s32Ret = ioctl(pstSnsState->stI2cInfo.fd, I2C_RDWR, &stI2cData);
    pthread_mutex_unlock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);
#endif

    if (s32Ret < 0) {
        cprintf("%s ret=%d,reg=0x%04x\n",
            pstSnsState->pcName, s32Ret, Reg);
        return s32Ret;
    }

    return EI_SUCCESS;
}

static EI_S32 CAMERA_I2C_WriteR8V16(SNS_STATE_S *pstSnsState, EI_U32 Reg, EI_U32 Val)
{
    EI_S32 s32Ret = EI_SUCCESS;
#if defined(__EOS__)

#else
    struct i2c_rdwr_ioctl_data stI2cData;
    struct i2c_msg stMsg = {0};
    EI_U8 Buf[3] = {(EI_U8)Reg, (EI_U8)(Val >> 8), (EI_U8)(Val & 0xff)};

    pthread_mutex_lock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);
    if (pstSnsState->bPower == EI_FALSE) {
        pthread_mutex_unlock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);
        PRT_VISS_WARN("%s sensor not power on!\n", pstSnsState->pcName);
        return -EI_FAILURE;
    }

    stMsg.addr  = pstSnsState->stI2cInfo.u16I2cDevAddr;
    stMsg.flags = 0;
    stMsg.len   = 3;
    stMsg.buf   = Buf;

    stI2cData.msgs = &stMsg;
    stI2cData.nmsgs = 1;

    s32Ret = ioctl(pstSnsState->stI2cInfo.fd, I2C_RDWR, &stI2cData);
    pthread_mutex_unlock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);
    if (s32Ret < 0) {
        cprintf("%s ret=%d,reg=0x%04x,val=0x%04x\n",
            pstSnsState->pcName, s32Ret, Reg, Val);
        return s32Ret;
    }
#endif
    return EI_SUCCESS;
}

static EI_S32 CAMERA_I2C_ReadR8V16(SNS_STATE_S *pstSnsState, EI_U32 Reg, void *pVal)
{
    EI_S32 s32Ret = EI_SUCCESS;
#if defined(__EOS__)

#else
    struct i2c_rdwr_ioctl_data stI2cData;
    struct i2c_msg stMsg[2] = {0};
    EI_U8 Tmp[2] = {0};

    pthread_mutex_lock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);
    if (pstSnsState->bPower == EI_FALSE) {
        pthread_mutex_unlock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);
        PRT_VISS_WARN("%s sensor not power on!\n", pstSnsState->pcName);
        return -EI_FAILURE;
    }

    stMsg[0].addr  = pstSnsState->stI2cInfo.u16I2cDevAddr;
    stMsg[0].flags = 0;
    stMsg[0].len   = 1;
    stMsg[0].buf   = (EI_U8 *)(&Reg);

    stMsg[1].addr  = pstSnsState->stI2cInfo.u16I2cDevAddr;
    stMsg[1].flags |= I2C_M_RD;
    stMsg[1].len   = 2;
    stMsg[1].buf   = Tmp;

    stI2cData.msgs = stMsg;
    stI2cData.nmsgs = 2;

    s32Ret = ioctl(pstSnsState->stI2cInfo.fd, I2C_RDWR, &stI2cData);
    pthread_mutex_unlock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);
    if (s32Ret < 0) {
        cprintf("%s ret=%d,reg=0x%04x\n",
            pstSnsState->pcName, s32Ret, Reg);
        return s32Ret;
    }

    *((EI_U16 *)pVal) = (Tmp[0] << 8) | Tmp[1];
#endif
    return EI_SUCCESS;
}

static EI_S32 CAMERA_I2C_WriteR16V16(SNS_STATE_S *pstSnsState, EI_U32 Reg, EI_U32 Val)
{
    EI_S32 s32Ret = EI_SUCCESS;
#if defined(__EOS__)
    struct rt_i2c_msg msgs[2];

#else
    struct i2c_rdwr_ioctl_data stI2cData;
    struct i2c_msg stMsg = {0};
#endif
    EI_U8 Buf[4] = { (EI_U8)(Reg >> 8), (EI_U8)(Reg & 0xff),
            (EI_U8)(Val >> 8), (EI_U8)(Val & 0xff)};
#if 0
    pthread_mutex_lock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);
    if (pstSnsState->bPower == EI_FALSE) {
        pthread_mutex_unlock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);
        cprintf("%s sensor not power on!\n", pstSnsState->pcName);
        return -EI_FAILURE;
    }
#endif
#if defined(__EOS__)

    pthread_mutex_lock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);

    if (pstSnsState->bPower == EI_FALSE) {
        PRT_VISS_WARN("%s sensor not power on!\n", pstSnsState->pcName);
        pthread_mutex_unlock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);
        return -EI_FAILURE;
    }
    msgs[0].addr  = pstSnsState->stI2cInfo.u16I2cDevAddr;
    msgs[0].flags = RT_I2C_WR;
    msgs[0].buf   = Buf;
    msgs[0].len   = 4;

    if (rt_i2c_transfer(pstSnsState->stI2cInfo.i2c_bus, msgs, 1) == 1)
        s32Ret = EI_SUCCESS;
    else
        s32Ret = EI_FAILURE;

    pthread_mutex_unlock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);

#else
    pthread_mutex_lock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);
    if (pstSnsState->bPower == EI_FALSE) {
        pthread_mutex_unlock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);
        PRT_VISS_WARN("%s sensor not power on!\n", pstSnsState->pcName);
        return -EI_FAILURE;
    }
    stMsg.addr  = pstSnsState->stI2cInfo.u16I2cDevAddr;
    stMsg.flags = 0;
    stMsg.len   = 4;
    stMsg.buf   = Buf;

    stI2cData.msgs = &stMsg;
    stI2cData.nmsgs = 1;

    s32Ret = ioctl(pstSnsState->stI2cInfo.fd, I2C_RDWR, &stI2cData);
    pthread_mutex_unlock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);
#endif

    if (s32Ret < 0) {
        cprintf("%s ret=%d,reg=0x%04x,val=0x%04x\n",
            pstSnsState->pcName, s32Ret, Reg, Val);
        return s32Ret;
    }

    return EI_SUCCESS;
}

static EI_S32 CAMERA_I2C_ReadR16V16(SNS_STATE_S *pstSnsState, EI_U32 Reg, void *pVal)
{
    EI_S32 s32Ret = EI_SUCCESS;
#if defined(__EOS__)
    struct rt_i2c_msg msgs[2];
#else
    struct i2c_rdwr_ioctl_data stI2cData;
    struct i2c_msg stMsg[2] = {0};
#endif
    EI_U8 Tmp[2] = {0};
    EI_U8 Buf[2] = {(EI_U8)(Reg >> 8), (EI_U8)(Reg & 0xff)};
#if 0
    pthread_mutex_lock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);
    if (pstSnsState->bPower == EI_FALSE) {
        pthread_mutex_unlock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);
        cprintf("%s sensor not power on!\n", pstSnsState->pcName);
        return -EI_FAILURE;
    }
#endif
#if defined(__EOS__)

    pthread_mutex_lock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);

    if (pstSnsState->bPower == EI_FALSE) {
        PRT_VISS_WARN("%s sensor not power on!\n", pstSnsState->pcName);
        pthread_mutex_unlock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);
        return -EI_FAILURE;
    }
    msgs[0].addr  = pstSnsState->stI2cInfo.u16I2cDevAddr;
    msgs[0].flags = RT_I2C_WR;
    msgs[0].buf   = Buf;
    msgs[0].len   = 2;

    msgs[1].addr  = pstSnsState->stI2cInfo.u16I2cDevAddr;
    msgs[1].flags = RT_I2C_RD; /* Read from slave */
    msgs[1].buf   = Tmp;
    msgs[1].len   = 2;

    if (rt_i2c_transfer(pstSnsState->stI2cInfo.i2c_bus, msgs, 2) == 2)
        s32Ret = EI_SUCCESS;
    else
        s32Ret = EI_FAILURE;


    pthread_mutex_unlock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);

#else
    pthread_mutex_lock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);
    if (pstSnsState->bPower == EI_FALSE) {
        pthread_mutex_unlock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);
        PRT_VISS_WARN("%s sensor not power on!\n", pstSnsState->pcName);
        return -EI_FAILURE;
    }
    stMsg[0].addr  = pstSnsState->stI2cInfo.u16I2cDevAddr;
    stMsg[0].flags = 0;
    stMsg[0].len   = 2;
    stMsg[0].buf   = Buf;

    stMsg[1].addr  = pstSnsState->stI2cInfo.u16I2cDevAddr;
    stMsg[1].flags |= I2C_M_RD;
    stMsg[1].len   = 2;
    stMsg[1].buf   = Tmp;

    stI2cData.msgs = stMsg;
    stI2cData.nmsgs = 2;

    s32Ret = ioctl(pstSnsState->stI2cInfo.fd, I2C_RDWR, &stI2cData);
    pthread_mutex_unlock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);
#endif

    if (s32Ret < 0) {
        cprintf("%s ret=%d,reg=0x%04x\n",
            pstSnsState->pcName, s32Ret, Reg);
        return s32Ret;
    }

    *((EI_U16 *)pVal) = (Tmp[0] << 8) | Tmp[1];

    return EI_SUCCESS;
}

static EI_S32 CAMERA_I2C_CqiWriteList(SNS_STATE_S *pstSnsState,
    const SNS_REG_S *RegsList, EI_U32 RegsCnt)
{
    EI_S32 s32Ret = EI_SUCCESS;
#if defined(__EOS__)

#else
    struct i2c_rdwr_ioctl_data stI2cData;
    struct i2c_msg stMsg = {0};
    EI_U16 u16I2cFlags = 0;

    u16I2cFlags = I2CCQI_USE |
    ((pstSnsState->stI2cInfo.u8RegBits == 8) ? 0 : I2CCQI_16BIT_REG) |
        ((pstSnsState->stI2cInfo.u8ValBits == 8) ? 0 : I2CCQI_16BIT_VAL);

    stMsg.addr  = pstSnsState->stI2cInfo.u16I2cDevAddr;
    stMsg.len = RegsCnt * 8;
    stMsg.flags = u16I2cFlags;
    stMsg.buf   = (EI_U8 *)RegsList;

    stI2cData.msgs = &stMsg;
    stI2cData.nmsgs = 1;

    s32Ret = ioctl(pstSnsState->stI2cInfo.fd, I2C_RDWR, &stI2cData);
    if (s32Ret) {
        cprintf("%s ret=%d CAMERA_I2C_CqiWriteList ioctl err\n",
            pstSnsState->pcName, s32Ret);
        return s32Ret;
    }
#endif
    return EI_SUCCESS;
}

static EI_S32 CAMERA_I2C_WriteRegList(SNS_STATE_S *pstSnsState,
    const SNS_REG_S *RegsList, EI_U32 RegsCnt)
{
    EI_S32 i;
    EI_S32 s32Ret = EI_SUCCESS;
#if defined(__EOS__)


#else
    EI_U32 u32I2cFuncs = 0;
    struct timeval stNowTime;
    struct timeval stOldTime;
    EI_CHAR nMode[10] = "normal";
    EI_CHAR cMode[10] = "cqi";
    EI_CHAR *pMode = nMode;
#endif
    if ((RegsList == NULL) || (RegsCnt == 0)) {
        return 0;
    }
#if defined(__EOS__)
    for (i = 0; i < RegsCnt; i++) {
        if (RegsList[i].u32Reg == SNS_REG_DELAY) {
            ei_msleep(RegsList[i].u32Val);
            continue;
        }
        s32Ret = pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, RegsList[i].u32Reg,
                RegsList[i].u32Val);
        if (s32Ret != 0) {
            cprintf("%s write %d reg 0x%04x val 0x%04x error\n",
                pstSnsState->pcName, i, RegsList[i].u32Reg,
                RegsList[i].u32Val);
            return s32Ret;
        }
    }

#else
    s32Ret = ioctl(pstSnsState->stI2cInfo.fd, I2C_FUNCS, &u32I2cFuncs);
    if (s32Ret < 0) {
        cprintf("%s ret=%d,get i2c func err\n",
            pstSnsState->pcName, s32Ret);
        return s32Ret;
    }

    if (u32I2cFuncs & I2C_FUNC_CQI) {
        pMode = cMode;
        gettimeofday(&stOldTime, EI_NULL);

        pthread_mutex_lock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);
        s32Ret = CAMERA_I2C_CqiWriteList(pstSnsState, RegsList, RegsCnt);
        pthread_mutex_unlock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);

        gettimeofday(&stNowTime, EI_NULL);

        if (s32Ret != 0) {
            cprintf("%s CAMERA_I2C_CqiWriteList  error\n", pstSnsState->pcName);
            return s32Ret;
        }
        goto OUT;
    }

    gettimeofday(&stOldTime, EI_NULL);
    for (i = 0; i < RegsCnt; i++) {
        if (RegsList[i].u32Reg == SNS_REG_DELAY) {
            ei_msleep(RegsList[i].u32Val);
            continue;
        }

        s32Ret = pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, RegsList[i].u32Reg,
                RegsList[i].u32Val);
        if (s32Ret != 0) {
            cprintf("%s write %d reg 0x%04x val 0x%04x error\n",
                pstSnsState->pcName, i, RegsList[i].u32Reg,
                RegsList[i].u32Val);
            return s32Ret;
        }
    }
    gettimeofday(&stNowTime, EI_NULL);

OUT:
    stNowTime.tv_sec = stNowTime.tv_sec - stOldTime.tv_sec;
    stNowTime.tv_usec = stNowTime.tv_usec - stOldTime.tv_usec;

#if 0 /* read camera init list */
    for (i = 0; i < RegsCnt; i++) {
        EI_U32 value = 0;

        s32Ret = pstSnsState->stI2cInfo.pfnReadReg(pstSnsState, RegsList[i].u32Reg,
                &value);
        if (value != RegsList[i].u32Val) {
            cprintf("%s read reg 0x%04x val 0x%04x ! = 0x%04x\n",
                pstSnsState->pcName, RegsList[i].u32Reg,
                RegsList[i].u32Val, value);
        }
    }
    cprintf("@@@ read cemra init list over\n");
#endif

    cprintf("%s %s init timer: %6lldms\n", pstSnsState->pcName, pMode,
        (long long)(stNowTime.tv_sec * 1000000 + stNowTime.tv_usec) / 1000);

#endif
    return EI_SUCCESS;
}


EI_S32 CAMERA_I2C_Reg_Ops(I2C_INFO_S *pstI2cInfo)
{
    EI_U32 reg_desc;
    EI_S32 s32Ret = EI_SUCCESS;

    reg_desc = (pstI2cInfo->u8RegBits << 8) | pstI2cInfo->u8ValBits;

    switch (reg_desc) {
    case SNS_REG_R8_V8_DESC:
        pstI2cInfo->pfnWriteReg = CAMERA_I2C_WriteR8V8;
        pstI2cInfo->pfnReadReg = CAMERA_I2C_ReadR8V8;
        break;
    case SNS_REG_R16_V8_DESC:
        pstI2cInfo->pfnWriteReg = CAMERA_I2C_WriteR16V8;
        pstI2cInfo->pfnReadReg = CAMERA_I2C_ReadR16V8;
        break;
    case SNS_REG_R8_V16_DESC:
        pstI2cInfo->pfnWriteReg = CAMERA_I2C_WriteR8V16;
        pstI2cInfo->pfnReadReg = CAMERA_I2C_ReadR8V16;
        break;
    case SNS_REG_R16_V16_DESC:
        pstI2cInfo->pfnWriteReg = CAMERA_I2C_WriteR16V16;
        pstI2cInfo->pfnReadReg = CAMERA_I2C_ReadR16V16;
        break;
    default:
        cprintf("Not supported comb of reg and val bits,error (%d, %d)\n",
            pstI2cInfo->u8RegBits, pstI2cInfo->u8ValBits);
        s32Ret = -1;
        break;
    }

    pstI2cInfo->pfnwriteRegList = CAMERA_I2C_WriteRegList;

    return s32Ret;
}

EI_S32 CAMERA_SensorPowerDown(VISS_DEV VissDev, SNS_STATE_S *pstSnsState)
{
    EI_S32 s32Ret = EI_SUCCESS;
    CAMERA_POWER_ACT_S s_sPowerDownAct[] = {
        {CAM_EN, SNS_DOWN},
    };
#if defined(__EOS__)


    pthread_mutex_lock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);
    pstSnsState->bPower = EI_FALSE;
    pthread_mutex_unlock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);

#else
    pthread_mutex_lock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);
    pstSnsState->bPower = EI_FALSE;
    pthread_mutex_unlock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);
#endif
    if (pstSnsState->pstPowerDownAct == NULL) {
        goto OUT;
    }

    s32Ret = VISS_SetPinState(VissDev, pstSnsState->pstPowerDownAct, pstSnsState->u8PwrDnActs);
    CAMERA_CHECK_SUCCESS(s32Ret);

    s32Ret = VISS_SetPinState(VissDev, s_sPowerDownAct, 1);
    CAMERA_CHECK_SUCCESS(s32Ret);

OUT:

    return s32Ret;
}

EI_S32 CAMERA_SensorPowerUp(VISS_DEV VissDev, SNS_STATE_S *pstSnsState)
{
    EI_S32 s32Ret = EI_SUCCESS;
    CAMERA_POWER_ACT_S s_sPowerUpAct[] = {
        {CAM_EN,  SNS_UP},
    };

    if (pstSnsState->pstPowerUpAct == NULL) {
        goto OUT;
    }

    s32Ret = VISS_SetPinState(VissDev, s_sPowerUpAct, 1);
    CAMERA_CHECK_SUCCESS(s32Ret);

    s32Ret = VISS_SetPinState(VissDev, pstSnsState->pstPowerUpAct, pstSnsState->u8PwrUpActs);
    CAMERA_CHECK_SUCCESS(s32Ret);

OUT:
#if defined(__EOS__)

    pthread_mutex_lock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);
    pstSnsState->bPower = EI_TRUE;
    pthread_mutex_unlock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);

#else
    pthread_mutex_lock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);
    pstSnsState->bPower = EI_TRUE;
    pthread_mutex_unlock(&g_aMutex[pstSnsState->stI2cInfo.u16I2cBusNum]);
#endif
    return s32Ret;
}

EI_S32 CAMERA_OpenI2c(SNS_STATE_S *pstSnsState)
{
    char acDevFile[16] = {0};
    EI_U8 u8DevNum;
    int ret;

    u8DevNum = pstSnsState->stI2cInfo.u16I2cBusNum;
#if defined(__EOS__)

    snprintf(acDevFile, sizeof(acDevFile),  "i2c%d", u8DevNum);

    pstSnsState->stI2cInfo.i2c_bus = rt_i2c_bus_device_find(acDevFile);
    if (NULL == pstSnsState->stI2cInfo.i2c_bus) {
        cprintf("Open i2c%d error!\n", u8DevNum);
        return EI_FAILURE;
    }
    pstSnsState->stI2cInfo.fd = 1;
#else
    snprintf(acDevFile, sizeof(acDevFile),  "/dev/i2c-%u", u8DevNum);

    if (pstSnsState->stI2cInfo.fd > 0) {
        cprintf("warning:Multiple open i2c%d\n", u8DevNum);
        return EI_SUCCESS;
    }

    pstSnsState->stI2cInfo.fd = open(acDevFile, O_RDWR, S_IRUSR | S_IWUSR);

    if (pstSnsState->stI2cInfo.fd < 0) {
        cprintf("Open /dev/i2c-%u error!\n", u8DevNum);
        return EI_FAILURE;
    } else {
        cprintf("Open /dev/i2c-%u successful!\n", u8DevNum);
    }

    ret = ioctl(pstSnsState->stI2cInfo.fd, I2C_SLAVE_FORCE, (pstSnsState->stI2cInfo.u16I2cDevAddr));
    if (ret < 0) {
        cprintf("I2C_SLAVE_FORCE error!\n");
        close(pstSnsState->stI2cInfo.fd);
        pstSnsState->stI2cInfo.fd = -1;
        return ret;
    }
#endif
    return EI_SUCCESS;
}

EI_S32 CAMERA_ExitI2c(SNS_STATE_S *pstSnsState)
{
#if defined(__EOS__)
    if (pstSnsState->stI2cInfo.i2c_bus) {
        pstSnsState->stI2cInfo.i2c_bus = NULL;
        pstSnsState->stI2cInfo.fd = -1;
        return EI_SUCCESS;
    }

#else

    if (pstSnsState->stI2cInfo.fd > 0) {
        close(pstSnsState->stI2cInfo.fd);
        pstSnsState->stI2cInfo.fd = -1;
        return EI_SUCCESS;
    }

#endif
    return EI_FAILURE;
}

#ifdef __cplusplus
#if __cplusplus
}
#endif
#endif /* __cplusplus */
